// =========================================================================
// ================ main.c - 最终重构版 (已修复作用域错误) ================
// =========================================================================
// 版本: 1.1 (修复编译报错与冲突)

// =================== 头文件区 ===================
#include "main.h"          // 包含项目全局声明
#include "sj_image.h"      // 包含图像处理模块的函数接口
#include "encoder.h"       // 编码器模块
#include "motor.h"         // 电机模块
#include "smotor.h"        // 舵机模块
#include "sj_pid.h"        // PID模块
#include "circle.h"        // 环岛状态机模块
#include "cross.h"         // 十字状态机模块
#include "sj_utils.h"      // 实用工具函数
#include "zebra.h"         // 新增：包含 zebra 状态机模块
#include <stdio.h>         // 用于 sprintf
#include <math.h>          // 用于 数学计算
#include <string.h>        // 用于 memcpy
#include "zf_host.h"       // 上位机模块


// =========================================================================
// =================== 全局变量定义区 ===================
// --- 核心控制变量 ---
float angle = 0.0f; // 舵机最终角度
float aim_distance = 0.5f; // 纯跟踪前瞻距离，影响灵敏度与平稳度
bool line_show_sample = true; // 是否显示采样点，方便调试
enum track_type_e track_type = TRACK_RIGHT; // 定义并初始化巡线模式

// --- 外部变量声明 (关键修复：必须放在这里，main函数才能看到) ---
extern float target_speed; 
extern float NORMAL_SPEED;         // 目标速度 (定义在 motor.c)
extern sf_pid_param_t motor_pid_l; // 左电机PID (定义在 motor.c)
extern sf_pid_param_t motor_pid_r; // 右电机PID (定义在 motor.c)
extern sf_pid_param_t servo_pid;   // 转向PID (定义在 smotor.c)
extern motor_param_t motor_l;      // 左电机状态 (定义在 motor.c)
extern motor_param_t motor_r;      // 右电机状态 (定义在 motor.c)

// --- 最终循迹线数据 ---
// 这是从 rptsc0 或 rptsc1 拷贝数据过来的最终目标
AT_DTCM_SECTION_ALIGN(float rptsn[POINTS_MAX_LEN][2], 8);
int rptsn_num = 0;

// --- 车辆启停状态机 ---
enum car_state_e {
    STATE_WAITING,
    STATE_RUNNING,
    STATE_STOPPING,
    STATE_FINISHED
};
enum car_state_e g_car_state = STATE_WAITING;   //初始化顶层车辆运行状态

#define MANUAL_START_BUTTON_PIN  (C13)          //按键启动，设置引脚
int64_t g_stop_encoder_start = 0;               //刚进入停车状态时，该变量负责记录此时车的总里程
const float STOP_DISTANCE_M = 1.0f;             //车辆在停车状态下行驶距离的标准，一旦超过该值切换为完全停车状态（finish）

// --- 调试显示用的缓冲区 ---
uint8_t g_display_buffer_large[240][376];
uint8_t g_display_buffer_raw_overlay[120][188];
uint8_t g_display_buffer_dual_view[240][188];


// =========================================================================
// ============================= 函数实现区 ==============================
// =========================================================================

//初始化所有系统模块
void Init()
{
    flash_init();   //flash初始化
    ips200_set_dir(IPS200_PORTAIT);
    ips200_init(IPS200_TYPE_SPI);

    //摄像头初始化
    while (mt9v03x_init())
    {
        ips200_show_string(0, 16, "mt9v03x reinit.");
        system_delay_ms(1000);
    }
    ips200_show_string(0, 16, "init success.");

    // 外设与算法模块的初始化
    encoder_init();     //编码器
    motor_init();       //电机驱动
    smotor_init();      //舵机控制
    sf_img_logic_init();      //图像处理逻辑
    zebra_logic_init();    // 初始化zebra 状态机
    
    //启动按键初始化
    gpio_init(MANUAL_START_BUTTON_PIN, GPI, GPIO_HIGH, GPI_PULL_UP);
    
    // 启动中断定时器
    pit_ms_init(PIT_CH0, 10);
}


/*
 * main()
 * ----------------------------------------------------------
 * 功能：系统主入口函数
 */
int main(void)
{
    //系统启动初始化
    clock_init(SYSTEM_CLOCK_600M);
    // debug_init();  // 【重要修改】注释掉它！防止占用 UART1 导致上位机无法使用
    system_delay_ms(300);

    Init();            //初始化所有模块
    interrupt_global_enable(0);

    ips200_show_string(0, 0, "State: WAITING");

    // ----------------- 上位机配置开始 -----------------
    ZF_Host_Init();                 // 初始化
    ZF_Host_Register(1, &NORMAL_SPEED);     // Ch1: 目标速度
    ZF_Host_Register(2, &aim_distance);     // Ch2: 前瞻距离
    ZF_Host_Register(3, &servo_pid.kp);     // Ch3: 转向KP (现在这里能看到 servo_pid 了)
    ZF_Host_Register(4, &servo_pid.kd);     // Ch4: 转向KD
    ZF_Host_Register(5, &motor_pid_l.kp);   // Ch5: 速度KP
    ZF_Host_Register(6, &motor_pid_l.ki);   // Ch6: 速度KI
    ZF_Host_Register(7, &motor_pid_l.kd);   // Ch7: 速度KD
    // ----------------- 上位机配置结束 -----------------

    //主循环
    while (1)
    {
        float aim_point_x = -1.0f;
        float aim_point_y = -1.0f;

        //每当摄像头完成一次数据采集，进行相应处理
        if (mt9v03x_finish_flag)
        {
            mt9v03x_finish_flag = 0;
            bool is_special_mode = false;

            // 1. 视觉处理
            sf_img_logic_process((uint8_t *)mt9v03x_image);
            
            // 2. zebra状态机更新
            check_zebra();   // 检查是否触发第一条斑马线
            run_zebra();     // 运行状态机逻辑
            
            bool is_button_pressed = (gpio_get_level(MANUAL_START_BUTTON_PIN) == 0);

            // 3. 启停状态机逻辑（顶层状态机）
            switch(g_car_state)
            {
                case STATE_WAITING:   //等待启动
                    target_speed = 0;
                    if(is_button_pressed){
                        if(is_button_pressed) {
                            system_delay_ms(30);
                            if(gpio_get_level(MANUAL_START_BUTTON_PIN) == 0) g_car_state = STATE_RUNNING;
                        } else { g_car_state = STATE_RUNNING; }
                        if(g_car_state == STATE_RUNNING) { ips200_show_string(0, 0, "State: RUNNING"); }
                    }
                    break;
                case STATE_RUNNING:   //正常行驶
                    // 斑马线停车判断逻辑
                    if (zebra_should_car_stop()) {
                        g_car_state = STATE_STOPPING;
                        ips200_show_string(0, 0, "State: STOPPING");
                        g_stop_encoder_start = get_total_encoder();
                    }
                    break;
                case STATE_STOPPING:    //正在停车
                    if(get_total_encoder() - g_stop_encoder_start > ENCODER_PER_METER * STOP_DISTANCE_M){
                        g_car_state = STATE_FINISHED;
                        ips200_show_string(0, 0, "State: FINISHED");
                    }
                    break;
                case STATE_FINISHED:    //完全停车
                    target_speed = 0;
                    break;
            }

            // 4. 元素状态机（具体策略的选择）
            if(g_car_state == STATE_RUNNING || g_car_state == STATE_STOPPING)
            {
                // a. 检查并运行特殊元素状态机
                check_cross();       //优先判断十字
                if (cross_type == CROSS_NONE) {
                    check_circle();  //非十字，再判断环岛
                }

                if (cross_type != CROSS_NONE) {
                    run_cross();
                    circle_type = CIRCLE_NONE;
                    is_special_mode = true;
                } else if (circle_type != CIRCLE_NONE) {
                    run_circle();
                    is_special_mode = true;
                }

                // b. 普通巡线模式
                if (!is_special_mode) {            
                    // 选择左右线数量较多的一侧作为主线
                    if (rptsc0_num > rptsc1_num) {   
                        track_type = TRACK_LEFT;
                        int copy_count = (rptsc0_num < POINTS_MAX_LEN) ? rptsc0_num : POINTS_MAX_LEN;
                        memcpy(rptsn, rptsc0, copy_count * sizeof(float) * 2);
                        rptsn_num = copy_count;
                    } else {
                        track_type = TRACK_RIGHT;
                        int copy_count = (rptsc1_num < POINTS_MAX_LEN) ? rptsc1_num : POINTS_MAX_LEN;
                        memcpy(rptsn, rptsc1, copy_count * sizeof(float) * 2);
                        rptsn_num = copy_count;
                    }
                }

                // 5. 纯跟踪算法控制舵机
                if (rptsn_num > 0) {    
                    // 默认使用全局前瞻距离
                    float current_aim_distance = aim_distance; 
                    
                    // 如果在十字入口的特殊状态，临时使用一个固定的远前瞻
                    if(cross_type == CROSS_BEGIN) {
                        current_aim_distance = 0.4f;
                    }

                    // 计算前瞻点索引
                    int aim_idx = clip(roundf(current_aim_distance / 0.02f), 0, rptsn_num - 1);

                    // 获取前瞻点的坐标（单位：像素）
                    aim_point_x = rptsn[aim_idx][0];
                    aim_point_y = rptsn[aim_idx][1];

                    // 纯跟踪计算
                    if (rptsn_num > 1 && aim_idx > 0) {
                        float dx = rptsn[aim_idx][0] - rptsn[0][0];
                        float dy = rptsn[aim_idx][1] - rptsn[0][1];
                        
                        float dn_pixel = sqrtf(dx * dx + dy * dy);
                        float dn_meter = dn_pixel / pixel_per_meter;
                        
                        if (dn_meter > 0.01f) { 
                            float dx_meter = dx / pixel_per_meter;
                            // 标准纯跟踪公式
                            float pure_angle = atan2f(2.0f * 0.21f * dx_meter, dn_meter * dn_meter) * 180.0f / PI;

                            // 最终PID控制舵机转向
                            angle = sf_pid_solve(&servo_pid, -pure_angle / SMOTOR_RATE);
                            steering_control(angle);
                        } else {
                            angle = 0; 
                            steering_control(angle);
                        }
                    } else { 
                        angle = 0;
                        steering_control(angle);
                    }
                } else { // 没有找到路径，舵机回中
                    angle = 0;
                    steering_control(angle);
                }
            }

            // 6. 调试信息显示
            
            // 步骤一：创建带扫线标记的原始图
            memcpy(g_display_buffer_raw_overlay, mt9v03x_image, sizeof(g_display_buffer_raw_overlay));
            image_t img_raw_overlay = DEF_IMAGE((uint8_t*)g_display_buffer_raw_overlay, 188, 120);
            
            int* raw_left_line = NULL; int raw_left_num = 0;
            int* raw_right_line = NULL; int raw_right_num = 0;
            sf_get_raw_left_line(&raw_left_line, &raw_left_num);
            sf_get_raw_right_line(&raw_right_line, &raw_right_num);
            
            for(int i = 0; i < raw_left_num; i++) {
                int x = raw_left_line[i*2+0], y = raw_left_line[i*2+1];
                if(x>=0&&x<188&&y>=0&&y<120) AT_IMAGE(&img_raw_overlay,x,y)=0;
            }
            for(int i = 0; i < raw_right_num; i++) {
                int x = raw_right_line[i*2+0], y = raw_right_line[i*2+1];
                if(x>=0&&x<188&&y>=0&&y<120) AT_IMAGE(&img_raw_overlay,x,y)=0;
            }

            // 步骤二：创建鸟瞰图并添加瞄点
            sf_draw_debug_view((uint8_t *)g_display_buffer_large);
            if (aim_point_x >= 0) { 
                image_t img_large_canvas = DEF_IMAGE((uint8_t*)g_display_buffer_large, 376, 240);
                draw_x(&img_large_canvas, (int)aim_point_x, (int)aim_point_y, 5, 255);
            }
            image_t img_large = DEF_IMAGE((uint8_t*)g_display_buffer_large, 376, 240);
            image_t img_small_top = DEF_IMAGE((uint8_t*)g_display_buffer_dual_view[0], 188, 120);
            downsample_2x_avg(&img_large, &img_small_top);

            // 步骤三：拼接图像
            memcpy(&g_display_buffer_dual_view[120][0], g_display_buffer_raw_overlay, sizeof(g_display_buffer_raw_overlay));
            
            // 步骤四：显示图像和文本
            ips200_show_gray_image(0, 0, (uint8_t *)g_display_buffer_dual_view, 188, 240, 188, 240, 0);

            const char* decision_mode_str = "NORM";
            if (cross_type != CROSS_NONE) {
                decision_mode_str = "CROS";
            } else if (circle_type != CIRCLE_NONE) {
                decision_mode_str = "CIRC"; 
            }

            char l1[40], l2[40], l3[40], l4[40];

            sprintf(l1, "L_Spd:%-5d R_Spd:%-5d", (int)motor_l.encoder_speed, (int)motor_r.encoder_speed);
            sprintf(l2, "T_Spd:%-5.1f Ang:%-5.1f", target_speed, angle);
            sprintf(l3, "Mod:%s S:%d Cr:%d Ci:%d", decision_mode_str, is_special_mode, cross_type, circle_type);
            sprintf(l4, "L:%-3d R:%-3d Aim:%-3d", rptsc0_num, rptsc1_num, rptsn_num);
            
            ips200_show_string(5, 242, l1);
            ips200_show_string(5, 261, l2);
            ips200_show_string(5, 280, l3);
            ips200_show_string(5, 299, l4);

            // 运行上位机任务：发送波形
            // 通道1: 目标速度, 通道2: 左轮实际, 通道3: 右轮实际, 通道4: 舵机角度
            ZF_Host_Task(target_speed, motor_l.encoder_speed, motor_r.encoder_speed, angle);

            // --- 同步参数：让右轮PID跟随左轮 ---
            // 这样你在上位机调 Ch5, 6, 7 时，左右轮参数会同时变
            motor_pid_r.kp = motor_pid_l.kp;
            motor_pid_r.ki = motor_pid_l.ki;
            motor_pid_r.kd = motor_pid_l.kd;
        }
    }
}